﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace YBOT_Field_Control
{
    /// <summary>
    /// Common Team Variables
    /// </summary>
    public class TeamVariables
    {
        public TowerVariables tower;    //Tower Class variable
        public string teamColor;        //Team color "red" or "green"
        public int score;               //Team's score
        public int nestEggs;            //Number of nest eggs
        public int panEggs;             //Number of pan eggs
        public int feedCount;           //Feed tower counter
        public int autoCount;           //Autonomous Mode Counter
        public int finalScore;          //Team's final score
        public int penalty;             //Penalty amount
        public bool dq;                 //Team DQ flag
        public string matchResult;      //Match Result string
        public bool autoFinished;       //Autonomous mode finished flag
        public bool candleStart;        //Candle mode start flag
        public CandleVariables candle;  //Candle class variable

        //Constructor
        public TeamVariables(string _color)
        {
            //Sets team color for this class
            this.teamColor = _color;
        }

        /// <summary>
        /// Set Team Data
        /// </summary>
        /// <param name="control_tower">Control Tower Number</param>
        /// <param name="candle_tower">Candle Tower Number</param>
        /// <param name="nest_tower">Nest Tower Number</param>
        /// <param name="pan_tower">Pan Tower Number</param>
        /// <param name="blue_feed_tower">Blue Feeder Tower Number</param>
        /// <param name="yellow_feed_tower">Yellow Feeder Tower Number</param>
        /// <param name="_ow">Pass One Wire Class</param>
        /// <param name="_xb">Pass XBee Class</param>
        public void setTowers(int control_tower, int candle_tower, int nest_tower, int pan_tower, int blue_feed_tower, int yellow_feed_tower, OneWire _ow, XBee _xb)
        {
            //Pass variables to Tower Class
            this.tower = new TowerVariables(control_tower, candle_tower, nest_tower, pan_tower, blue_feed_tower, yellow_feed_tower, _ow);
            //Pass variables to Candle Class
            this.candle = new CandleVariables(tower, _ow, _xb, this.teamColor);
        }

        /// <summary>
        /// Resets variable
        /// </summary>
        public void reset()
        {
            this.score = 0;
            this.nestEggs = 0;
            this.panEggs = 0;
            this.nestEggs = 0;
            this.feedCount = 0;
            this.autoCount = 0;
            this.finalScore = 0;
            this.penalty = 0;
            this.dq = false;
            this.matchResult = null;
            this.autoFinished = false;
            this.candleStart = false;
            this.tower.reset();
            this.candle.reset();
        }
    }

    /// <summary>
    /// Common Team Tower Variables 
    /// </summary>
    public class TowerVariables
    {
        public int controlTower;            //Control tower number
        public int controllerRelay = 4;     //Controller relay number - Enable / Disable
        public int auto_driverRelay = 5;    //Autonomous Mode relay = Auto / Manual
        public int redLight = 1;            //Red light relay
        public int greenLight = 2;          //Green light relay
        public int blueLight = 4;           //Blue light relay
        public int yellowLight = 3;         //Yellow light relay
        public int redInput= 6;             //Red button input
        public int greenInput = 6;          //Green button input
        public int blueInput = 7;           //Blue button input
        public int yellowInput = 8;         //Yellow button input
        public int candleTower;             //Candle tower number
        public int nestTower;               //Nest tower number
        public int panTower;                //Pan tower number
        public int blueFeedTower;           //Blue Feeder tower number
        public int yellowFeedTower;         //Yellow tower number
        public int randomTower = 0;         //Random tower number
        public int randomLight = 0;         //Random light relay
        public bool redPress = false;       //Red button flag - true if pressed
        public bool greenPress = false;     //Green button flag - true if pressed
        public bool bluePress = false;      //Blue button flag - true if pressed
        public bool yellowPress = false;    //Yellow button flag - true if pressed
        OneWire t1w;                        //One wire variable for this class
        
        /// <summary>
        /// Sets Tower Class Variables
        /// </summary>
        /// <param name="control_tower">Control Tower number</param>
        /// <param name="candle_tower">Candle Tower number</param>
        /// <param name="nest_tower">Nest Tower number</param>
        /// <param name="pan_tower">Pan Tower number</param>
        /// <param name="blue_feed_tower">Blue Feeder Tower number</param>
        /// <param name="yellow_feed_tower">Yellow Feeder Tower number</param>
        /// <param name="_ow">Pass One Wire Class</param>
        public TowerVariables(int control_tower, int candle_tower, int nest_tower, int pan_tower, int blue_feed_tower, int yellow_feed_tower, OneWire _ow)
        {
            this.controlTower = control_tower;
            this.candleTower = candle_tower;
            this.nestTower = nest_tower;
            this.panTower = pan_tower;
            this.blueFeedTower = blue_feed_tower;
            this.yellowFeedTower = yellow_feed_tower;
            this.t1w = _ow;
        }

        /// <summary>
        /// Flip to opposite feeder tower
        /// </summary>
        public void flipFeedTower()
        {
            int currentTower = this.randomTower;                                                    //Current tower = current random tower
            this.t1w.AllRelaysOff(currentTower);                                                    //Turn off all relays on current tower
            if (currentTower == this.blueFeedTower) this.randomTower =  this.yellowFeedTower;       //If tower is blue set random tower to yellow
            else this.randomTower = this.blueFeedTower;                                             //Else set random tower to blue    
            this.randomLight = this.randomNum(this.yellowLight, this.blueLight);                    //Select random light
            this.t1w.RelayOn(this.randomLight, this.randomTower);                                   //Turn on random light on random tower
            this.t1w.ResetActivityLatch(this.randomTower);                                          //Clear Activity latches on tower
        }

        /// <summary>
        /// Check to see if button is pressed
        /// </summary>
        /// <param name="towerNum">Tower to check</param>
        /// <returns>Returns true if pressed</returns>
        public bool buttonPress(int towerNum)
        {
            bool pressed = false;                                   //set pressed flag to false
            if (this.t1w.Status(this.redInput, towerNum, true))     //If the red button has been pressed
            {
                this.redPress = true;                               
                pressed = true;                                     
            }
            else this.redPress = false;

            if (this.t1w.Status(this.greenInput, towerNum, true))   //If the green button has been pressed
            {
                this.greenPress = true;
                pressed = true;
            }
            else this.greenPress = false;

            if (this.t1w.Status(this.blueInput, towerNum, true))    //If the blue button has been pressed
            {
                this.bluePress = true;
                pressed = true;
            }
            else this.bluePress = false;

            if (this.t1w.Status(this.yellowInput, towerNum, true))  //If the yellow button has been pressed
            {
                this.yellowPress = true;
                pressed = true;
            }
            else this.yellowPress = false;

            return pressed;
        }

        /// <summary>
        /// Random number generator
        /// </summary>
        /// <param name="lowNum">Low Number</param>
        /// <param name="highNum">High Number</param>
        /// <returns>Returns random interger the falls within Low and High Number</returns>
        private int randomNum(int lowNum, int highNum)
        {
            int i = DateTime.Now.Second;                //Random Number Seed
            Random newNum = new Random(i);              //New Random Number
            return newNum.Next(lowNum, highNum + 1);    //Return Random Number
        }

        /// <summary>
        /// Resets Tower flags
        /// </summary>
        public void reset()
        {
            //this.controlTower = 0;
            //this.candleTower = 0;
            //this.nestTower = 0;
            //this.panTower = 0;
            //this.blueFeedTower = 0;
            //this.yellowFeedTower = 0;       
            this.randomTower = 0;         
            this.randomLight = 0;         
            this.redPress = false;
            this.greenPress = false;
            this.bluePress = false;
            this.yellowPress = false;
        }
    }

    /// <summary>
    /// This class holds all common methods for the Candling Mode
    /// </summary>
    public class CandleVariables
    {
        public string teamColor;        //Team's color
        public bool descore;            //Descore flag
        public int panEggCount;         //Current Egg Count for pan tower
        public int nestEggCount;        //Current Egg Count for nest tower
        public int panEggTotal;         //Total number eggs allowed under pan tower
        public int nestEggTotal;        //Total number eggs allowed under nest tower
        public string panOverride;      //Pan tower override value
        public string nestOverride;     //Nest tower override value
        public int perfectScore = 1;    //perfectscore tracker
        public int randomEggTower;      //Random tower to place eggs under
        private int eggMax = 6;         //Max number of eggs that can be placed under a tower
        public bool eggsCounted;        //Have the eggs been counted flag
        private int previousCount = 0;      //Last Pixel Count
        public int currentNestCount = 0;    //Current Number of eggs
        public int currentPanCount = 0;

        TowerVariables t;               //Passed TowerVariables 
        OneWire c1w;                    //Passed One Wire
        XBee cXB;                       //Passed XBee
            
        /// <summary>
        /// Constructor for CandleVariables
        /// </summary>
        /// <param name="_tower">Pass Tower Class</param>
        /// <param name="_oneWire">Pass One Wire Class</param>
        /// <param name="_xbee">Pass XBee Class</param>
        /// <param name="_color">Pass Team Color</param>
        public CandleVariables(TowerVariables _tower, OneWire _oneWire, XBee _xbee, string _color)
        {
            this.t = _tower;
            this.c1w = _oneWire;
            this.cXB = _xbee;
            this.teamColor = _color;
        }

        /// <summary>
        /// Clears current Candle Mode Values
        /// </summary>
        public void reset()
        {
            //this.teamColor = null;
            this.descore = false;
            this.panEggCount = 0;
            this.nestEggCount = 0;
            this.panEggTotal = 0;
            this.nestEggTotal = 0;
            this.panOverride = null;
            this.nestOverride = null;
            this.randomEggTower = 0;
            this.eggsCounted = true;
            this.currentNestCount = 0;
            this.currentPanCount = 0;
            this.perfectScore = 1;
            this.eggMax = 6;
        }

        /// <summary>
        /// Turns off egg towers and turn on candling tower
        /// </summary>
        public void candleReset()
        {
            if (this.t.randomTower != 0)                        //If tower is not 0
            {
                this.c1w.AllRelaysOff(this.t.randomTower);      //Turn off all relays
            }
            if (this.randomEggTower != 0)                       //If random egg tower is not 0
            {
                this.AllRelaysOffSafe(this.randomEggTower);     //Turn off all relays
            }

            int relay = 0;                                      //Null relay

            if (this.teamColor == "red")                        //If team is red
            {
                relay = this.t.redLight;                        //Relay = red light relay
            }
            else if (this.teamColor == "green")                 //If team is green
            {
                relay = this.t.greenLight;                      //Relay = green light relay
            }

            //If egg total for pan or nest tower does not equal max number of allowed eggs
            if (this.panEggTotal != this.eggMax || this.nestEggTotal != this.eggMax)
            {
                c1w.RelayOn(relay, this.t.candleTower);         //Turn on candle tower relay
                c1w.ResetActivityLatch(this.t.candleTower);     //Reset activity status on candle tower
            }
        }

        /// <summary>
        /// Candles the egg and pick a random tower to deposit the egg.
        /// </summary>
        public void candleEgg()
        {
            this.AllRelaysOffSafe(this.t.candleTower);      //Turn off all relays on candle tower
            chooseTower();                                  //Choose random corner tower
            eggsCounted = false;                            //Reset eggsCounted flag
        }

        /// <summary>
        /// Selects random egg tower and feeder tower
        /// </summary>
        public void chooseTower()
        {
            int relay = 0;                                  //Nulls relay

            if (this.teamColor == "red")                    //If team is red
            {
                relay = this.t.redLight;                    //relay = red light relay
            }
            else if (this.teamColor == "green")             //If team is green
            {
                relay = this.t.greenLight;                  //relay = green ligth relay
            }

            //If pan and nest egg totals are < the max number of allowed eggs
            if (this.panEggTotal < this.eggMax && this.nestEggTotal < this.eggMax)
            {
                int i = randomNum(1, 2);                            //get random number

                if (i == 2)                                         //If random number is 2
                {
                    this.randomEggTower = this.t.panTower;          //randomEggTower = pan tower number
                    this.t.randomTower = this.t.blueFeedTower;      //randomTower = blue feeder tower number
                    this.t.randomLight = this.t.blueLight;          //randomLight = blue light relay
                    this.panEggTotal++;                             //Increment Pan Egg Total
                }
                else                                                //Else it is 1
                {
                    this.randomEggTower = this.t.nestTower;         //randomEggTower = nest tower number
                    this.t.randomTower = this.t.yellowFeedTower;    //randomTower = yellow feeder tower number
                    this.t.randomLight = this.t.yellowLight;        //randomLight = yellow light relay
                    this.nestEggTotal++;                            //Increment Nest Egg Total
                }
            }
            //Else if nest is at max number of allowed eggs and pan is not
            else if (this.panEggTotal < this.eggMax && this.nestEggTotal == this.eggMax)
            {
                this.randomEggTower = this.t.panTower;          //randomEggTower = pan tower number
                this.t.randomTower = this.t.blueFeedTower;      //randomTower = blue feeder tower number
                this.t.randomLight = this.t.blueLight;          //randomLight = blue light relay
                this.panEggTotal++;                             //Increment Pan Egg Total
            }
            //Else if pan is at max number of allowed eggs and nest is not
            else if (this.panEggTotal == this.eggMax && this.nestEggTotal < this.eggMax)
            {
                this.randomEggTower = this.t.nestTower;         //randomEggTower = nest tower number
                this.t.randomTower = this.t.yellowFeedTower;    //randomTower = yellow feeder tower number
                this.t.randomLight = this.t.yellowLight;        //randomLight = yellow light relay
                this.nestEggTotal++;                            //Increment Nest Egg Total
            }
            //Else Both are done
            else this.randomEggTower = 0;

            //If Egg tower is not 0
            if (this.randomEggTower != 0)
            {
                this.c1w.RelayOn(relay, this.randomEggTower);               //Turn on randomEggTower lights
                this.c1w.RelayOn(this.t.randomLight, this.t.randomTower);   //Turn on random feeder tower lights
                this.c1w.ResetActivityLatch(this.t.randomTower);            //Reset random feeder tower activity latches
            }
        }

        /// <summary>
        /// Counts the eggs at the active tower
        /// </summary>
        /// <returns>returns true</returns>
        public int countEggs()
        {
            int towerTotal = 0;                                     //nulls tower total
            this.eggsCounted = true;                                //change eggs counted flag to true
            this.descore = false;                                   //Change descore to false

            if (this.randomEggTower == this.t.panTower)             //If randomEggTower is pan tower
            {
                towerTotal = this.panEggTotal;                      //towerTotal = pan tower's egg total
            }
            else if (this.randomEggTower == this.t.nestTower)       //If randomEggTower is nest tower
            {
                towerTotal = this.nestEggTotal;                     //towerTotal = nest tower's egg total
            }
            else return 0;                                          //Else return 0

            int eggs = this.eggCounter(this.randomEggTower, towerTotal);      //eggs = counted eggs

            if (eggs <= towerTotal)                                 //If eggs are <= tower total
            {
                if (eggs == towerTotal) this.perfectScore++;        //If the egg count is correct then increament perfectScore
                return eggs;                                        //Return number of eggs
            }
            else if (eggs > towerTotal)                             //If eggs > than tower total
            {
                descore = true;                                     //Descore = true
                return 0;                                           //Eggs = zero
            }

            else return 0;                                          //Else return 0
        }

        /// <summary>
        /// Used to get the current number of eggs under a given tower
        /// </summary>
        /// <param name="TowerNum">Number of tower to check</param>
        /// <returns>Returns the number of eggs</returns>
        public int eggCounter(int TowerNum, int expectedNum)
        {

            //If the given tower is the pan tower and the override value is not LIVE get the override value
            if (this.panOverride != "LIVE" && this.randomEggTower == this.t.panTower)
            {
                if (this.panOverride != "")
                {
                    int i = Convert.ToInt16(this.panOverride);       //i = pan override value
                    return i;                                   //return i
                }
                else return 0;
            }
            //If the given tower is the nest tower and the override value is not LIVE get the override value
            else if (this.nestOverride != "LIVE" && this.randomEggTower == this.t.nestTower)
            {
                if (this.nestOverride != "")
                {
                    int i = Convert.ToInt16(this.nestOverride);      //i = nest override value
                    return i;                                   //return i
                }
                else return 0;
            }
   
            //Else return 0
            else return 0;
        }

        /// <summary>
        /// Used to create a random number
        /// </summary>
        /// <param name="lowNum">lowest value</param>
        /// <param name="highNum">highest value</param>
        /// <returns></returns>
        private int randomNum(int lowNum, int highNum)
        {
            int i = DateTime.Now.Second;                //Random number seed
            Random newNum = new Random(i);              //new random number
            return newNum.Next(lowNum, highNum + 1);    //Return random number
        }

        /// <summary>
        /// Used to turn off all Relays on all towers without changing the robot controller or arduino relay state
        /// </summary>
        public void AllRelaysOffSafe(int towerNum)
        {
            byte relayBuffer = 0;                                           //Set buffer all relays off
            relayBuffer = c1w.BuildRelayState(4, 1, relayBuffer);           //Set relay state: all off controller/reset relay on 
            relayBuffer = c1w.BuildRelayState(5, 1, relayBuffer);           //Set relay state: all off controller and driver on

            c1w.SetTowerState(relayBuffer, towerNum);                       //Change tower to current state
        }

    }
}
